using System;
using System.Collections.Generic;
using System.Text;
using Cambia.CoreLib;

using LocalizationTest.Tools;
using System.Drawing;

namespace LocalizationTest
{
  public class DistanceSensorAmbiguous : Sensor
  {
    MapWithLandmarks.LandMarkClasses landmarkClass;
    MapWithLandmarks.LandmarkIDs landmarkID;
    double headingDirection;
    double angleOfView;

    public DistanceSensorAmbiguous()
    {
      this.landmarkClass = MapWithLandmarks.LandMarkClasses.corner;
      this.headingDirection = 0;
      this.angleOfView = Math.PI / 2.0;
    }

    public DistanceSensorAmbiguous(MapWithLandmarks.LandMarkClasses landmarkClass)
    {
      this.landmarkClass = landmarkClass;
      this.headingDirection = 0;
      this.angleOfView = Math.PI / 3.0;
    }

    public DistanceSensorAmbiguous(MapWithLandmarks.LandMarkClasses landmarkClass, double headingDirection, double angleOfView)
    {
      this.landmarkClass = landmarkClass;
      this.headingDirection = headingDirection;
      this.angleOfView = angleOfView;
    }

    internal override void SimulateMeasurement(WorldState worldState, Map map, ActionAndObservationHistory actionAndObservationHistory)
    {
      for (int i = 0; i < (map as MapWithLandmarks).XPositionOfLandmark.Length; i++)
      {
        landmarkID = (MapWithLandmarks.LandmarkIDs)i;
        if (MapWithLandmarks.GetLandmarkClass(landmarkID) == landmarkClass)
        {
          Measurement performedMeasurement = PredictForSimulation(worldState, map);
          actionAndObservationHistory.Add(performedMeasurement);
        }
      }
      /*
      if (alt == 0)
      {
        alt = 1;
      }
      else
      {
        alt = 0;
      }
      landmarkID = (MapWithLandmarks.LandmarkIDs)alt;
      Measurement performedMeasurement = PredictForSimulation(worldState, map);
      actionAndObservationHistory.Add(performedMeasurement);
       */
    }

    public override double GetMeasurementDeviation(WorldState worldState, Map map, Measurement measurement)
    {
      double minDifference = double.MaxValue;
      double averageDifference = 0;
      int averageCounter = 0;
      for (int i = 0; i < (map as MapWithLandmarks).XPositionOfLandmark.Length; i++)
      {
        landmarkID = (MapWithLandmarks.LandmarkIDs)i;
        if (MapWithLandmarks.GetLandmarkClass(landmarkID) == landmarkClass)
        {
          Measurement prediction = measurement.sensor.PredictPerfect(worldState, map);
          double difference = prediction.GetDifference(measurement);
          if (Math.Abs(difference) < Math.Abs(minDifference)) minDifference = difference;
          averageDifference += difference;
          averageCounter++;
        }
      }
      averageDifference /= averageCounter;
      return minDifference;
      //return averageDifference;
    }


    public override Measurement PredictForSimulation(WorldState worldState, Map map)
    {
      bool limitView = DebugKeyTool.IsActive("_measurement:limited view");
      double sigma = Globals.distanceMeasurementSigma;
      double gaussianError = RandomProvider.NextNormal() * sigma; // Math.Sqrt(sigma);
      double systematicError = 20.0;

      switch (Globals.selfLocatorProperties.SensorNoise)
      {
        case NoiseType.equal: break; //todo
        case NoiseType.noNoise: gaussianError = 0; systematicError = 0; break;
        case NoiseType.gaussianAndSystematic: break;
        case NoiseType.gaussianNoSystematic: systematicError = 0; break;
        case NoiseType.noGaussianButSystematic: gaussianError = 0; break;
      }
      DistanceMeasurement measurement = (DistanceMeasurement)PredictPerfect2(worldState, map, limitView);
      if (measurement == null)
        return null;
      else
        return new DistanceMeasurement(measurement.Distance + gaussianError + systematicError, this);
    }

    public override Measurement PredictPerfect(WorldState worldState, Map map)
    {
      return PredictPerfect2(worldState, map, false);
    }
    private Measurement PredictPerfect2(WorldState worldState, Map map, bool limitView)
    {
      double distance = 0;

      double x_robot = 0;
      double y_robot = 0;
      double theta_robot = 0;
      double x_landmark = 0;
      double y_landmark = 0;

      if (worldState is WorldState1D)
      {
        x_robot = (worldState as WorldState1D).Position;
      }

      if (worldState is WorldState2D)
      {
        x_robot = (worldState as WorldState2D).Vector.x;
        y_robot = (worldState as WorldState2D).Vector.y;
      }

      if (worldState is WorldStatePose2D)
      {
        x_robot = (worldState as WorldStatePose2D).Pose.translation.x;
        y_robot = (worldState as WorldStatePose2D).Pose.translation.y;
        theta_robot = (worldState as WorldStatePose2D).Pose.rotation;
      }

      if (map is MapWithLandmarks)
      {
        x_landmark = (map as MapWithLandmarks).XPositionOfLandmark[(int)landmarkID];
        y_landmark = (map as MapWithLandmarks).YPositionOfLandmark[(int)landmarkID];
      }

      distance = Math.Sqrt(Math.Pow(y_landmark - y_robot,2) + Math.Pow(x_landmark - x_robot, 2) );

      double angle = Math.Atan2(y_landmark - y_robot, x_landmark - x_robot) - theta_robot;
      if (Math.Abs(MathTools.Normalize(angle - headingDirection)) < angleOfView / 2.0 || !limitView)
        return new DistanceMeasurement(distance, this);
      else
        return null;

    }

    public override double GetMeasurementProbability(WorldState worldState, Map map, Measurement measurement)
    {
      double expectedMeasurement = (PredictPerfect(worldState, map) as DistanceMeasurement).Distance;
      double conductedMeasurement = (measurement as DistanceMeasurement).Distance;
      return Globals.Gaussian(conductedMeasurement, Globals.distanceMeasurementSigma, expectedMeasurement);
    }

    public override void Draw(Measurement measurement, WorldState worldState, double shade, string drawingID)
    {
      double distance = (measurement as DistanceMeasurement).Distance;
      double startX = 0;
      double startY = 0;
      if (worldState is WorldState1D)
      {
        startX = (worldState as WorldState1D).Position;
        startY = 0;
      }
      if (worldState is WorldState2D)
      {
        startX = (worldState as WorldState2D).Vector.x;
        startY = (worldState as WorldState2D).Vector.y;
      }
      if (worldState is WorldStatePose2D)
      {
        startX = (worldState as WorldStatePose2D).Pose.translation.x;
        startY = (worldState as WorldStatePose2D).Pose.translation.y;
      }
      Color color = Color.FromArgb((int)(255.0), (int)(200.0 * shade), (int)(200.0 * shade));
      double width = 5;
      if (shade == 0.9999) { color = Color.Gray; width = 1; }
      DebugDrawingTool.CreateCircle(drawingID, new Vector2(startX, startY), distance, width, color);
      DebugDrawingTool.CreateLine(drawingID, new Vector2(startX - 15, startY), new Vector2(startX + 15, startY), width, color);
      DebugDrawingTool.CreateLine(drawingID, new Vector2(startX, startY - 15), new Vector2(startX, startY + 15), width, color);
    }

    public override string ToString()
    {
      return this.GetType().Name + ";" + landmarkClass.ToString();
    }
  }
}
